
function MainStep3_MultiShoot
%
% 多步打靶（LEO -> LLO）
% 1）LEO和LLO用轨道根数表示
%
% 参考
% [1] 张晨，基于数值延拓的日月综合借力DRO 入轨策略
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2022年02月24日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc; clear; close all; clear path;
% restoredefaultpath;
warning off;

% 添加模块
parentFolder = fileparts(pwd);
addpath(genpath([parentFolder , '\Basic']));
addpath(genpath([parentFolder , '\Bcr4bp']));
addpath(genpath([parentFolder , '\Cr3bp']));

% 载入参数
aux = Bcr4bp_Aux;

% 载入上一步结果
load picked_LEO2DRO;

% ---------------------------- 计算DRO轨道 ----------------------------
aux = get_DRO(aux);

% --------------------------------------------------------------------------------------------------------------------------
% 构造多步打靶初值
yy = getIC_Coe(picked_LEO2DRO , aux);

opts = optimoptions(@fmincon , 'Algorithm' , 'active-set' , 'Display' , 'iter' , ...
    'MaxIter' , 20 , 'MaxFunEvals' , 200 , ...
    'GradObj' , 'on' , 'GradConstr' , 'on' , 'DerivativeCheck' , 'off');
[yy , fobj , ExitFlag] = fmincon(@Bcr4bp_Obj , yy , [] , [] , [] , [] , [] , [] , @Bcr4bp_ConsEMRot2D_Coe , opts , aux);

% 画图
xx1 = Bcr4bp_var2xxEMRot2D(yy(1 : 4) , 'leo' , aux)';
% xxf = yy(4 * (aux.nNode - 1) + 1 : 4 * aux.nNode);
tau1 = yy(4 * aux.nNode + 1);
tauf = yy(4 * aux.nNode + 2);
sigma = yy(4 * aux.nNode + 3);

% EMRot轨道
options = odeset('Reltol' , aux.tol , 'AbsTol' , aux.tol);
[seg_tt , seg_xx] = ode113(@Bcr4bp_EqmEMRot2D , [tau1 , tauf] , [xx1] , options , aux);
Bcr4bp_ScnEMRot(tau1 , aux);
plotOrb2D(seg_xx , 'r' , 2)

% SB1Rot轨道
figure(2);
[tau_SB1Rot , xb2c_SB1Rot] = Bcr4bp_EMRot2SB1RotMtx2D(seg_tt , seg_xx , aux);
Bcr4bp_ScnSB1Rot(tau_SB1Rot(end) , aux);
plotOrb2D(xb2c_SB1Rot , 'r' , 2);

% 保存结果
converged_LEO2DRO = [seg_tt(1) , seg_xx(1 , :) , seg_tt(end) , seg_xx(end , :) , sigma];
save converged_LEO2DRO converged_LEO2DRO

end
